# 三角网坐标平差 Triangular mesh coordinate adjustment
from MathUtils import *
import numpy as np

# 点名和边名
A = 'A'
B = 'B'
C = 'C'
D = 'D'
P1 = 'P1'
P2 = 'P2'

P1A = 'P1A'
P1B = 'P1B'
P1C = 'P1C'
P1P2 = 'P1P2'
P2A = 'P2A'
P2C = 'P2C'
P2D = 'P2D'

AB = 'AB'
BC = 'BC'
CD = 'CD'
DA = 'DA'

p = 206265


class Point:
	known = True

	def __init__(self, x, y):
		self.x = x
		self.y = y

	def tostring(self):
		return "x: {}, y: {}".format(self.x, self.y)


class Side:
	ahc = [0, 0, 0, 0]

	def __init__(self, p1, p2, s=-1.0, a=114514.0):
		# 点名, 坐标, 边长, 坐标方位角
		self.point1 = p1
		self.point2 = p2
		if s != -1:
			self.s = s
			self.s2 = s * s
			self.dx = p2.x - p1.x
			self.dy = p2.y - p1.y
		else:
			self.dx = p2.x - p1.x
			self.dy = p2.y - p1.y
			self.s2 = self.dx * self.dx + self.dy * self.dy
			self.s = np.sqrt(self.s2)

		if a != 114514.0:
			self.a = a
		# print("{} = {}".format(azimuth(p1, p2), a))
		else:
			self.a = azimuth(p1, p2)

	def tostring(self):
		d, m, s = dec2dms(self.a)
		return "起始点 p1: [{}]\n终止点 p2: [{}]\n[dx: {}, dy: {}]\n边长 s: {}\n边长² s2: {}\n坐标方位角 a: {}°{}'{}\"\n坐标方位角改正数各项系数 ahc: {}\t{}\t{}\t{}".format(
			self.point1.tostring(), self.point2.tostring(), self.dx, self.dy, self.s, self.s2, d, m, s, self.ahc[0],
			self.ahc[1], self.ahc[2], self.ahc[3])


class Angle:
	a = 0.0
	anti = False
	tria = 0.0

	def __init__(self, side1, side2, obtuse=False):
		self.s2 = side2
		self.s1 = side1
		# self.a = self.s2.a - self.s1.a
		self.a = side2.a - side1.a
		self.tria = self.triA(obtuse)

	def triA(self, isObtuse):
		tria = self.a
		if 180 <= tria < 360:
			tria -= 180

		if isObtuse:
			# 求钝角
			if tria >= 90:
				return tria
			else:
				self.anti = True
				return 180 - tria
		else:
			# 求锐角
			if tria >= 90:
				self.anti = True
				return 180 - tria
			else:
				return tria


def formulate7_22_2(pointA, pointB, LA, LB):
	LA = np.deg2rad(LA)
	LB = np.deg2rad(LB)
	XA = pointA.x
	XB = pointB.x
	YA = pointA.y
	YB = pointB.y
	x = (XA * cot(LB) + XB * cot(LA) - YB + YA) / (cot(LA) + cot(LB))
	y = (YA * cot(LB) + YB * cot(LA) + XB - XA) / (cot(LA) + cot(LB))
	return Point(x, y)


def updateSites(tags, endpoints, P, S):
	for t, e in zip(tags, endpoints):
		v = Side(P[e[0]], P[e[1]])
		ec = [0, 0, 0, 0]
		a = (p * v.dy) / v.s2
		b = -(p * v.dx) / v.s2
		if P1 in e and P2 in e:
			ec[0] = a
			ec[1] = b
			ec[2] = -a
			ec[3] = -b
		else:
			if P1 in e:
				ec[0] = a
				ec[1] = b
			if P2 in e:
				ec[2] = a
				ec[3] = b
		v.ahc = ec
		S[t] = v


if __name__ == '__main__':
	# 读取角度观测值
	L = []
	with open("OeTriangularMeshCoordAdj_AngleObservations.txt", "r") as f:
		while True:
			line = f.readline()
			if line == "":
				break
			d, m, s = line.split(",")
			d = float(d)
			m = float(m)
			s = float(s)
			L.append(dms2dec((d, m, s)))
		f.close()

	# 近似点集合
	P = {}
	# 近似边集合
	S = {}

	# 起始数据加入近似点集合
	P[A] = Point(9684.28, 43836.82)
	P[B] = Point(10649.55, 31996.50)
	P[C] = Point(19063.66, 37818.86)
	P[D] = Point(17814.63, 49923.19)
	# 起始数据加入近似边集合
	S[AB] = Side(P[A], P[B], 11879.60, dms2dec((274, 39, 38.4)))
	S[BC] = Side(P[B], P[C], 10232.16, dms2dec((34, 40, 56.3)))
	S[CD] = Side(P[C], P[D], 12168.60, dms2dec((95, 53, 29.1)))
	S[DA] = Side(P[D], P[A], 10156.11, dms2dec((216, 49, 6.5)))

	# 计算近似坐标,加入近似点集合
	# 根据所选三角形的不同, 近似坐标略有差异
	# P1近似坐标
	P[P1] = formulate7_22_2(P[B], P[C], L[18 - 1], L[16 - 1])
	# P[P1] = Point(13188.61, 37334.97)
	P[P1].known = False
	# P2近似坐标
	P[P2] = formulate7_22_2(P[C], P[D], L[7 - 1], L[9 - 1])
	# P[P2] = Point(15578.61, 44391.03)
	P[P2].known = False
	print("近似点坐标: ")
	print("P1: ", P[P1].tostring())
	print("P2: ", P[P2].tostring())

	# 计算近似边, 加入近似边集合
	tags = [P1A, P1B, P1C, P1P2, P2A, P2C, P2D]
	endpoints = [(P1, A), (P1, B), (P1, C), (P1, P2), (P2, A), (P2, C), (P2, D)]
	updateSites(tags, endpoints, P, S)

	# 输出每一条边
	for k, v in S.items():
		print("边名: ", k)
		print(v.tostring())
		print("{0:-^20}".format(""))

	# 构建角集合
	# 每一个角按照其边的坐标方位角大小,小角度边输入tags1,大角度边输入tags2,若该边为钝角obtus设置为True,否则为False
	A = []
	# 		1		2		3		4		5		6		7		8		9		10		11		12		13		14		15		16		17		18
	tags1 = [P1A, 	P1A, 	P1B, 	P2D, 	P2A, 	P2D, 	CD, 	P2D, 	P2D, 	P1P2, 	P1A, 	P1P2, 	P1P2, 	P1C, 	P1C, 	P1C, 	P1C, 	BC]
	tags2 = [P1B, 	AB, 	AB, 	P2A, 	DA, 	DA, 	P2C, 	P2C, 	CD, 	P2A, 	P2A, 	P1A, 	P2C, 	P1P2, 	P2C, 	BC, 	P1B, 	P1B]
	obtus = [True, 	False, 	False, 	True, 	False, 	False, 	False, 	True, 	False, 	False, 	False, 	False, 	False, 	False, 	False,	False, 	True, 	False]
	for t1, t2, obt in zip(tags1, tags2, obtus):
		angle = Angle(S[t1], S[t2], obt)
		print(dec2dms(angle.tria))
		A.append(angle)

	# 误差方程系数阵
	B = np.zeros([18, 4])
	# 坐标方位角近似值
	L0 = np.zeros([18, 1])
	# 坐标方位角观测值
	Lv = np.zeros([18, 1])
	# 误差方程数据初始化
	for r in range(0, 18):
		for c in range(0, 4):
			if A[r].anti:
				B[r][c] = A[r].s1.ahc[c] - A[r].s2.ahc[c]
			else:
				B[r][c] = A[r].s2.ahc[c] - A[r].s1.ahc[c]
		Lv[r][0] = L[r]
		L0[r][0] = A[r].tria
	# 误差方程常数
	l = Lv - L0
	print("坐标方位角观测值 Lv = \n", Lv)
	print("坐标方位角近似值 L0 = \n", L0)
	print("误差方程常数 l = \n", l)
	print("误差方程系数阵 B = \n", B)

	# 法方程系数阵
	Nbb = B.transpose().dot(B)
	# 法方程常数
	W = B.transpose().dot(l)
	# 坐标改正数
	xh = np.linalg.inv(Nbb).dot(W)
	# 角度改正数
	V = B.dot(xh) - l
	# 观测量平差值
	Lh = Lv + V
	print("角度改正数 V = \n", V)
	print("观测量平差值 Lh = \n", Lh)
	print("坐标改正数 xh = \n", xh)
	# 使用坐标改正数更新近似点坐标
	P[P1].x += xh[0][0]
	P[P1].y += xh[1][0]
	P[P2].x += xh[2][0]
	P[P2].y += xh[3][0]
	print("坐标平差值:")
	print(P[P1].tostring())
	print(P[P2].tostring())

	# 待定边平差后的集合
	Sh = {}
	updateSites(tags, endpoints, P, Sh)
	# 输出待定边的坐标方位角和边长平差值
	for k, v in Sh.items():
		print("边名: ", k)
		print(v.tostring())
		print("{0:-^20}".format(""))

	# 单位权中误差
	sigma_0h = np.sqrt((V.transpose().dot(V)) / (18 - 4))
	d, m, s = dec2dms(sigma_0h[0][0])
	print("单位权中误差 = {}\"".format(s))
	# Qxx
	Qxx = np.linalg.inv(Nbb)
	# 待定点坐标中误差
	sigma_x1h = sigma_0h * np.sqrt(Qxx[0][0])
	sigma_y1h = sigma_0h * np.sqrt(Qxx[1][1])
	sigma_p1h = np.sqrt(sigma_x1h * sigma_x1h + sigma_y1h * sigma_y1h)
	sigma_x2h = sigma_0h * np.sqrt(Qxx[2][2])
	sigma_y2h = sigma_0h * np.sqrt(Qxx[3][3])
	sigma_p2h = np.sqrt(sigma_x2h * sigma_x2h + sigma_y2h * sigma_y2h)
	print("各待定点坐标中误差: ")
	print("sigma_x1h = {} m".format(sigma_x1h[0][0]))
	print("sigma_y1h = {} m".format(sigma_y1h[0][0]))
	print("sigma_p1h = {} m".format(sigma_p1h[0][0]))
	print("sigma_x2h = {} m".format(sigma_x2h[0][0]))
	print("sigma_y2h = {} m".format(sigma_y2h[0][0]))
	print("sigma_p2h = {} m".format(sigma_p2h[0][0]))
